#include <cmath>
#include <iostream>
#include <thread>

#include "asio_driver/device/buzzer_src.hpp"
#include "asio_driver/device/rc_sbus.hpp"
#include "asio_driver/io/timer.hpp"
#include "asio_driver/utils/range.hpp"
#include "asio_driver_special/imu_wit_cmp10a.hpp"
#include "asio_driver_special/motor_brt_can.hpp"
//#include "asio_driver_special/motor_dc_pwm.hpp"
#include "asio_driver_special/motor_dc_io.hpp"
#include "ctrl_alg/filter/window_filter.hpp"
#include "ctrl_alg/pid/pid_pos.hpp"

static constexpr int rate = 200;

#include "chassis/recorder.hpp"

unsigned char connectCommand[2] = "c";
unsigned char disconnectCommand[2] = "d";

int main()
{
    Recorder recorder;
    recorder.SetPath("../../recorder.csv");

    std::cout << "init device!" << std::endl;
    AD::AsioContext::SharePtr context = AD::AsioContext::CreatAsioContext();
    std::unique_ptr<AD::Can> can = std::make_unique<AD::Can>(context);
    AD::CanBus::SharePtr can_bus = AD::CanBus::Create(context);
    AD::Special::MotorBrtCan motorLeft = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorRight = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorLeftForward = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorRightForward = AD::Special::MotorBrtCan(context);

    AD::Special::IMUWitCmp10a imu = AD::Special::IMUWitCmp10a(context);
    AD::RC_Sbus sbus = AD::RC_Sbus(context);
    AD::Buzzer buzzer = AD::Buzzer(context);
    // AD::Special::MotorDCPwm motorLeftConnect = AD::Special::MotorDCPwm(context);
    // AD::Special::MotorDCPwm motorRightConnect = AD::Special::MotorDCPwm(context);
    AD::Special::MotorDC_Mode1 connectLeft = AD::Special::MotorDC_Mode1(context);
    AD::Special::MotorDC_Mode1 connectRight = AD::Special::MotorDC_Mode1(context);

    motorLeft.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 4, .poleCount = 14});
    motorRight.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 3, .poleCount = 14});
    motorLeftForward.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 1, .poleCount = 14});
    motorRightForward.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{.address = 2, .poleCount = 14});

    can->Open("can0");
    can_bus->Open(std::move(can));
    imu.Open("/dev/ttyS6", 115200);
    sbus.Open("/dev/ttyS7");
    buzzer.Open(34, true);
    // motorLeftConnect.Open(AD::Special::MotorDCPwm::Config{.pinDirection = 36, .pinPWM = 39});
    // motorRightConnect.Open(AD::Special::MotorDCPwm::Config{.pinDirection = 40, .pinPWM = 42});
    connectLeft.Open(
        AD::Special::MotorDC_Mode1::Config{.pinDirection = 63, .pinBrake = 62, .pinFeedback = 101, .pinPWM = 47});
    connectRight.Open(
        AD::Special::MotorDC_Mode1::Config{.pinDirection = 113, .pinBrake = 46, .pinFeedback = 35, .pinPWM = 114});

    CtrlAlg::PIDPos<double> gyroPID = CtrlAlg::PIDPos<double>(300, 0, 0);   // 200
    CtrlAlg::PIDPos<double> anglePID = CtrlAlg::PIDPos<double>(5, 0, 0);    // 5
    CtrlAlg::PIDPos<double> speedPID = CtrlAlg::PIDPos<double>(10, 0.1, 0); // 10-0.1
    speedPID.SetMaxIntegralOutputPart(3);
    CtrlAlg::PIDPos gyroZPID = CtrlAlg::PIDPos(40, 0, 0); // 50
    CtrlAlg::WindowFilter<double> speedFilter = CtrlAlg::WindowFilter<double>(40);

    buzzer.Beep();
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    buzzer.StopBeep();
    std::cout << "start control!" << std::endl;

    //* pid self-blance
    recorder.CreatTitle({"time", "gyro", "gyroZ", "angle", "speed", "targetSpeed", "targetAngle", "targetGyro",
                         "targetGyroZ", "targetCurrent"});
    int speedTick = 0;
    int angleTick = 0;
    int canTick = 0;

    float targetAngle = 0;
    float targetGyro = 0;
    float forwardVal = 0;
    float angle = 0;
    float gyro = 0;
    float gyroZ = 0;

    bool checkRc = true;

    int attr = fcntl(STDIN_FILENO, F_GETFL);
    attr |= O_NONBLOCK;
    fcntl(STDIN_FILENO, F_SETFL, attr);
    char key[] = "s";

    while (context->Running())
    {
        context->SpinOnce();
        // int len = read(STDIN_FILENO, &key, 2);
        // if (len > 0)
        //{
        //     if (key[0] == 'c')
        //     {
        //         connectLeft.SetDirection(AD::Special::MotorDC_Mode1::Direction::Clockwise);
        //         connectRight.SetDirection(AD::Special::MotorDC_Mode1::Direction::Clockwise);
        //         std::cout << "connect" << std::endl;
        //     }
        //     else if (key[0] == 'd')
        //     {
        //         connectLeft.SetDirection(AD::Special::MotorDC_Mode1::Direction::CounterClockwise);
        //         connectRight.SetDirection(AD::Special::MotorDC_Mode1::Direction::CounterClockwise);
        //         std::cout << "disconnect" << std::endl;
        //     }
        //     else
        //     {
        //         connectLeft.SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        //         connectRight.SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        //         std::cout << "stop" << std::endl;
        //     }
        // }
        // continue;
        if (checkRc)
        {
            auto rcFrame = sbus.ReadFrame();
            if (rcFrame.IsDisconnect())
            {
                std::cerr << "sbus disconnect" << std::endl;
                continue;
            }
            auto dirCtrl = rcFrame.ModeChannel(4);
            auto connect = rcFrame.ModeChannel(5);
            if (dirCtrl != AD::RC_Sbus::Frame::ModeChannelValue::Down)
            {
                std::cerr << "sbus mode <left set> not reset" << std::endl;
                continue;
            }
            if (connect != AD::RC_Sbus::Frame::ModeChannelValue::Centrel)
            {
                std::cerr << "sbus mode <right set> not reset" << std::endl;
                continue;
            }
            checkRc = false;
            std::cout << "sbus mode reset, check finished" << std::endl;
        }
        // Get input
        auto rcFrame = sbus.ReadFrame();
        float speedCtrl = rcFrame.Channel(3, 0, 1);
        auto dirCtrl = rcFrame.ModeChannel(4);
        auto connect = rcFrame.ModeChannel(5);
        float forwardControl = rcFrame.Channel(1, -0.5, 0.5);
        float rotateControl = rcFrame.Channel(0, -30, 30);
        auto imuData = imu.Read();
        gyroZ = AD::Utils::InRange(imuData.gyro[2], -60, 60) ? -imuData.gyro[2] : gyroZ;
        gyro = AD::Utils::InRange(imuData.gyro[0], -30, 30) ? -imuData.gyro[0] : gyro;
        angle = AD::Utils::InRange(imuData.angle[0], -30, 30) ? -(imuData.angle[0] - 1.6) : angle;
        // motorLeft.RequestRPM();
        // motorRight.RequestRPM();
        float speed = (motorLeft.ReadState().rpm + motorRight.ReadState().rpm) / 2 / 60 * 2 * 3.1415926 * 0.12;
        speed = speedFilter.GetControl(speed);
        // motorLeft.RequestCurrent();
        // motorRight.RequestCurrent();
        // float current = (motorLeft.ReadState().current + motorRight.ReadState().current) / 2;
        recorder.SaveTime();
        recorder.SaveFloat(gyro);
        recorder.SaveFloat(gyroZ);
        recorder.SaveFloat(angle);
        recorder.SaveFloat(speed);
        // recorder.SaveFloat(current);

        // Check Range
        // if (std::abs(angle) > 13)
        //{
        //    std::cerr << "angle overlap: " << angle << std::endl;
        //    return 0;
        //}
        // if (std::abs(speed) > 1)
        //{
        //    std::cerr << "speed overlap: " << speed << std::endl;
        //    return 0;
        //}

        // connect motor
        switch (connect)
        {
        case AD::RC_Sbus::Frame::ModeChannelValue::Up:
            // motorLeftConnect.SetControl(AD::Special::MotorDCPwm::Direction::CounterClockwise);
            // motorRightConnect.SetControl(AD::Special::MotorDCPwm::Direction::CounterClockwise);
            connectLeft.SetDirection(AD::Special::MotorDC_Mode1::Direction::CounterClockwise);
            connectRight.SetDirection(AD::Special::MotorDC_Mode1::Direction::CounterClockwise);

            canTick++;
            if (canTick > 100)
            {
                canTick = 0;
                can_bus->Send(0x0f, std::span<const unsigned char>(disconnectCommand, disconnectCommand + 2));
            }
            break;
        case AD::RC_Sbus::Frame::ModeChannelValue::Centrel:
            // motorLeftConnect.SetControl(AD::Special::MotorDCPwm::Direction::Stop);
            // motorRightConnect.SetControl(AD::Special::MotorDCPwm::Direction::Stop);
            connectLeft.SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            connectRight.SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            break;
        case AD::RC_Sbus::Frame::ModeChannelValue::Down:
            // motorLeftConnect.SetControl(AD::Special::MotorDCPwm::Direction::Clockwise);
            // motorRightConnect.SetControl(AD::Special::MotorDCPwm::Direction::Clockwise);
            connectLeft.SetDirection(AD::Special::MotorDC_Mode1::Direction::Clockwise);
            connectRight.SetDirection(AD::Special::MotorDC_Mode1::Direction::Clockwise);
            canTick++;
            if (canTick > 100)
            {
                canTick = 0;
                can_bus->Send(0x0f, std::span<const unsigned char>(connectCommand, connectCommand + 2));
            }
            break;
        case AD::RC_Sbus::Frame::ModeChannelValue::ErrorNotMode:
            break;
        }
        // Speed PID
        speedTick++;
        if (speedTick > 20)
        {
            speedTick = 0;
            targetAngle = speedPID.GetControl(forwardControl - speed, 1000.0 / rate);
            targetAngle = AD::Utils::ClampAbs(targetAngle, 10);
        }
        // Angle PID
        angleTick++;
        if (angleTick > 5)
        {
            angleTick = 0;
            targetGyro = anglePID.GetControl(targetAngle - angle, 1000.0 / rate);
        }
        // Gyro PID
        float forwardVal = -gyroPID.GetControl(targetGyro - gyro, 1000.0 / rate);
        forwardVal = AD::Utils::ClampAbs(forwardVal, 6000);

        // GyroZ PID
        // float gyroZVal = gyroZPID.GetControl(std::copysign(rotateControl, speedCtrl) - gyroZ, 1000.0 / rate);
        float gyroZVal = gyroZPID.GetControl(rotateControl - gyroZ, 1000.0 / rate);
        gyroZVal = AD::Utils::ClampAbs(gyroZVal, 2000);

        recorder.SaveFloat(forwardControl);
        recorder.SaveFloat(targetAngle);
        recorder.SaveFloat(targetGyro);
        recorder.SaveFloat(rotateControl);
        recorder.SaveFloat(forwardVal); // current

        float left = 0;
        float right = 0;
        // Set Motor
        switch (dirCtrl)
        {
            // 4WD
        case AD::RC_Sbus::Frame::ModeChannelValue::Up:
            forwardVal = forwardControl * 6000;
            left = forwardVal + rotateControl * 200;
            right = forwardVal - rotateControl * 200;
            left = AD::Utils::ClampAbs(left, 10000);
            right = AD::Utils::ClampAbs(right, 10000);
            motorLeft.SendHeartBeat();
            motorRight.SendHeartBeat();
            motorLeftForward.SendHeartBeat();
            motorRightForward.SendHeartBeat();
            motorLeft.SetCurrent(left);
            motorRight.SetCurrent(right);
            motorLeftForward.SetCurrent(left);
            motorRightForward.SetCurrent(right);
            break;
        case AD::RC_Sbus::Frame::ModeChannelValue::Centrel:
            // DC
            forwardVal = forwardControl * 2000;
            left = forwardVal + gyroZVal;
            right = forwardVal - gyroZVal;
            left = AD::Utils::ClampAbs(left, 4000);
            right = AD::Utils::ClampAbs(right, 4000);
            motorLeft.SendHeartBeat();
            motorRight.SendHeartBeat();
            motorLeft.SetCurrent(left);
            motorRight.SetCurrent(right);
            break;
            // BAL
        case AD::RC_Sbus::Frame::ModeChannelValue::Down:
            left = forwardVal + gyroZVal;
            right = forwardVal - gyroZVal;
            left = AD::Utils::ClampAbs(left, 10000);
            right = AD::Utils::ClampAbs(right, 10000);
            // motorLeft.SendHeartBeat();
            // motorRight.SendHeartBeat();
            // motorLeft.SetCurrent(left);
            // motorRight.SetCurrent(right);
            break;
        default:
            break;
        }

        recorder.NewLine();
        context->WaitRate(rate);
    }
    //*/
    recorder.Save();
    return 0;
}